package clubhouse.control;

import java.awt.Color;
import java.io.FileOutputStream;
import java.io.IOException;
import java.io.OutputStreamWriter;
import java.text.DecimalFormat;
import java.util.ListIterator;
import java.util.Vector;

import javax.swing.text.Style;
import javax.swing.text.StyleConstants;

import clubhouse.base.Base;
import clubhouse.error.TransmissionError;

public class DataChecker {
	public static final int initialPosition = 1;
	public static final int infoReceive = 2;
	public static final int error = 3;
	public static final int robotEstimate = 4;
	public static final int landmark = 5;
	public static final int destination = 6;
	public static final int moveInfo = 7;
	public static final int movePulses = 8;
	public static final int newPosition = 9;
	public static final int stopInfo = 10;
	public static final int antiSlam = 11;
	public static final int ekf = 12;
	public static final int displayM1 = 13;
	public static final int displayM2 = 14;
	public static final int displayM3 = 15;
	public static final int displayM4 = 16;
	
	public static final double communicationRange = 20;
	public static final int sensingRange = 10;
	public static final int actingRange = 5;
	
	public int errorFactor;
	
	private Style currentStyle;
	private int step;
	int indexStep=0;
	private Vector<double[]> realRobotPositions;
	private Vector<double[]> realDestination;
	private double[] currentRobotPosition;
	private int dataError;
	private double rootMeanSquareError;
	private Vector<double[]> landmarks;	//for each landmark : [nodeid,x,y,M1nodeid]
	private boolean doCheck;
	private int scenario,M3=-1;
	private double M2=0,C=-1;
	private int [] M4;
	private DecimalFormat df;

	public DataChecker(Style style, boolean doCheck) {
		this.currentStyle = style;
		this.doCheck = doCheck;
		this.clear();
		this.M4 = new int[]{-1,0};
		this.df = new DecimalFormat("#####.##"); 
	}
	
	public void setErrorFactor(int errorFactor) {
		this.errorFactor = errorFactor;
	}
	
	public double[] getLandmark(int nodeid) {
		double[] temp = null, result = null;
        ListIterator<double[]> li = this.landmarks.listIterator();
        while(li.hasNext()){
        	temp = li.next();
        	if (temp[0] == nodeid) {
        		result = temp;
        		break;
        	}
        }
		return result;
	}
	
	public void setDoCheck(boolean doCheck, int scenario) {
		this.doCheck = doCheck;
		this.scenario = scenario;
		this.landmarks = new Vector<double[]>(5,1);
	}

	public void initRobotPositions(){
		this.realRobotPositions = new Vector<double[]>(90,90);
		for (int i=0;i<90;i++){
			if (i<=30){
				this.realRobotPositions.add(new double[]{i-10,20});
			}
			if ((i>30)&&(i<=50)){
				this.realRobotPositions.add(new double[]{20,20-i+30});
			}
			if ((i>50)&&(i<=70)){
				this.realRobotPositions.add(new double[]{20-i+50,0});
			}
			if ((i>70)&&(i<=90)){
				this.realRobotPositions.add(new double[]{0,0+i-70});
			}
		}
	}
	
	private void initRealDestination(){
		this.realDestination = new Vector<double[]>(4,4);
		this.realDestination.add(new double[]{0,20});
		this.realDestination.add(new double[]{20,20});
		this.realDestination.add(new double[]{20,0});
		this.realDestination.add(new double[]{0,0});
	}

	public Style getCurrentStyle() {
		return currentStyle;
	}
	
	public int getLandmarkIndex(int nodeid) {
		double[] temp = null;
		int result = -1;
		
        for (int i=0;i<this.landmarks.size();i++){
        	temp = this.landmarks.get(i);
        	if ((temp != null)&&(temp[0] == nodeid)) {
        		result = i;
        		break;
        	}
        }
		return result;
	}
	
	private String isPlurial(double data){	
		if (data>1){ 
			return "s";
		} else {
			return "";
		}
	}
	
	private double euclideanDist(double x1, double y1, double x2, double y2){
		return(Math.sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1)));
	}

	public String verifyData(int type,double... dataPreview){
		String result = "";
		double[] data=null;
		if (dataPreview !=null){
			data = new double[dataPreview.length];
			System.arraycopy(dataPreview, 0, data, 0, dataPreview.length);
		}
		StyleConstants.setForeground(this.currentStyle, Color.black);
		double dist;
		
		switch (type){
			case DataChecker.initialPosition:
				result = "[Initial Position]";
				result = result+"\tx: "+df.format(data[0]);
				result = result+"\ty: "+df.format(data[1]);
				result = result+"\tr: "+df.format(data[2]);
				this.currentRobotPosition[0] = data[0];
				this.currentRobotPosition[1] = data[1];
				
				if (this.scenario == 1) {
					if (!this.doCheck) break;
					dist = this.euclideanDist(this.realRobotPositions.get(indexStep)[0], this.realRobotPositions.get(indexStep)[1], this.currentRobotPosition[0], this.currentRobotPosition[1]);
					if (dist>=0.5){
						StyleConstants.setForeground(this.currentStyle, Color.red);
						this.dataError++;
						result = result+"\tinstead of ("+this.realRobotPositions.get(indexStep)[0]+";"+this.realRobotPositions.get(indexStep)[1]+")";
					} else if (dist>=0.05){
						StyleConstants.setForeground(this.currentStyle, Color.gray);
					}
					rootMeanSquareError = rootMeanSquareError+dist;
				} else {
					
				}
				break;
				
			case DataChecker.infoReceive:
				result = "[Data Received]";
				result = result+"\tnode "+df.format(data[0]);
				result = result+"\tx: "+df.format(data[1]);
				result = result+"\ty: "+df.format(data[2]);
				break;
				
			case DataChecker.antiSlam:
				result = "[antiSlamed node "+df.format(data[0]);
				result = result+"]\t("+df.format(data[1]);
				result = result+";"+df.format(data[2]);
				result = result+")\tbecome\t("+df.format(data[3]);
				result = result+";"+df.format(data[4])+")";
				break;
				
			case DataChecker.error:
				double maxError = this.euclideanDist(data[1], data[2], this.currentRobotPosition[0], this.currentRobotPosition[1])/10;
				dist = this.euclideanDist(data[1],data[2],data[3],data[4]);
				//System.out.println("(DataChecker)\tdist = "+dist+", maxError="+maxError);
				result = "[Error node "+df.format(data[0]);
				result = result+"]\t("+df.format(data[1]);
				result = result+";"+df.format(data[2]);
				result = result+")\tbecome\t("+df.format(data[3]);
				result = result+";"+df.format(data[4])+")";
				
				if (!this.doCheck) break;
				if ((dist>maxError)){
					StyleConstants.setForeground(this.currentStyle, Color.red);
					this.dataError++;
				}
				break;
				
			case DataChecker.robotEstimate:
				result = "[Robot Estimate]";
				result = result+"\tx: "+df.format(data[0]);
				result = result+"\ty: "+df.format(data[1]);
				result = result+"\tr: "+df.format(data[2]);
				break;
				
			case DataChecker.landmark:
				double[] tempLandmark = null;
				
				result = "[Landmark "+df.format(data[0])+" on "+df.format(data[1])+"]";
				result = result+"\tx: "+df.format(data[2]);
				result = result+"\ty: "+df.format(data[3]);
				
				int tempIndex = this.getLandmarkIndex((int)data[0]);
				if (tempIndex==-1){
					this.landmarks.add(new double[]{(int)data[0],data[2],data[3],this.euclideanDist(this.currentRobotPosition[0], this.currentRobotPosition[1], data[2], data[3])});
				} else {
					tempLandmark = this.landmarks.get(tempIndex);
					dist = this.euclideanDist(this.currentRobotPosition[0], this.currentRobotPosition[1], data[2], data[3]);
					if ((dist<DataChecker.sensingRange)&&(this.M4[0]==-1)) {
						this.M4[0] = this.step;
						this.M4[1] = (int) data[0];
					}
					if (dist>tempLandmark[3]) dist = tempLandmark[3];
					
					if (this.C==-1) C = dist;
					else if (dist < this.C) C = dist;
					
					this.landmarks.remove(tempIndex);
					this.landmarks.add(new double[]{(int)data[0],data[2],data[3],dist});
				}
				
				break;
				
			case DataChecker.destination:
				double [] realDest = null;
				result = "[Destination]\t\tx: "+df.format(data[0]);
				result = result+"\ty: "+df.format(data[1]);
				if ((indexStep>=0)&&(indexStep<10)) realDest = this.realDestination.get(0);
				if ((indexStep>=10)&&(indexStep<30)) realDest = this.realDestination.get(1);
				if ((indexStep>=30)&&(indexStep<50)) realDest = this.realDestination.get(2);
				if ((indexStep>=50)&&(indexStep<70)) realDest = this.realDestination.get(3);
				if ((indexStep>=70)&&(indexStep<90)) realDest = this.realDestination.get(0);
				
				if (!this.doCheck) break;
				dist = this.euclideanDist(realDest[0], realDest[1], data[0], data[1]);
				if (dist > this.euclideanDist(data[0], data[1],this.currentRobotPosition[0], this.currentRobotPosition[1])) {
					StyleConstants.setForeground(this.currentStyle, Color.red);
					this.dataError++;
					result = result + "\tinstead of ("+realDest[0]+","+realDest[1]+")";
				} else if (dist>=0.05) StyleConstants.setForeground(this.currentStyle, Color.gray);
				
				break;
				
			case DataChecker.moveInfo:
				result = "[Move's Info]\t\td: "+df.format(data[0]);
				result = result+" unit"+this.isPlurial(data[0])+" \tr: "+df.format(data[1]);

				if (data[1]==0)	result = result+" rad";
				else { 
					if (data[2]==0){
						result = result+" rad in non-trigonometric direction";
					} else {
						result = result+" rad in trigonometric direction";
					}
				}
				
				if (!this.doCheck) break;
				//r checks
				if ( ((indexStep==10)&&this.step!=10) || (indexStep==30) || (indexStep==50) || (indexStep==70) ){	//steps where r should be -1.57
					if (( Math.abs(Math.abs(data[1])-Math.PI/2) > 0.5 ) || (data[2]!=0)){
						StyleConstants.setForeground(this.currentStyle, Color.red);
						this.dataError++;
						result = result + "\tr wrong, should be 1,57 rad in non-trigonometric direction";
					} else if (Math.abs(Math.abs(data[1])-Math.PI/2) > 0.01) StyleConstants.setForeground(this.currentStyle, Color.gray);
				} else {		//other steps where r should be 0
					if ( Math.abs(data[1]) > 0.5 ){	
						StyleConstants.setForeground(this.currentStyle, Color.red);
						this.dataError++;
						result = result + "\tr wrong, should be 0 rad";
					} else if ( Math.abs(data[1]) > 0.01 ) StyleConstants.setForeground(this.currentStyle, Color.gray);
				}
				break;
				
			case DataChecker.movePulses:
				result = "[Move's Pulses]\td: "+df.format(data[0]);
				result = result+" pulse"+this.isPlurial(data[0])+" \tr: "+df.format(data[1]);
				if (data[1]==0)	result = result+" pulse";
				else { 
					if (data[2]==0){
						result = result+" pulse"+this.isPlurial(data[1])+" in non-trigonometric direction";
					} else {
						result = result+" pulse"+this.isPlurial(data[1])+" in trigonometric direction";
					}
				}
				
				if (!this.doCheck) break;
				//r checks
				if ( ((indexStep==10)&&this.step!=10) || (indexStep==30) || (indexStep==50) || (indexStep==70) ){	//steps where r should be -1.57
					if (( Math.abs(Math.abs(data[1])-50) > 0.5*100/Math.PI ) || (data[2]!=0)){
						StyleConstants.setForeground(this.currentStyle, Color.red);
						this.dataError++;
						result = result + "\tr wrong, should be 50 pulses in non-trigonometric direction";
					} else if (Math.abs(Math.abs(data[1])-50) > 0.01*100/Math.PI) StyleConstants.setForeground(this.currentStyle, Color.gray);
				} else {		//other steps where r should be 0
					if ( Math.abs(data[1]) > 0.5*100/Math.PI ){	
						StyleConstants.setForeground(this.currentStyle, Color.red);
						this.dataError++;
						result = result + "\tr wrong, should be 0 pulse";
					} else if ( Math.abs(data[1]) > 0.01*100/Math.PI ) StyleConstants.setForeground(this.currentStyle, Color.gray);
				}				
				break;
				
			case DataChecker.newPosition:
				result = "[New Position]\tx: "+df.format(data[0]);
				result = result+"\ty: "+df.format(data[1]);
				result = result+"\tr: "+df.format(data[2])+"\n";
				
				if (this.scenario != 1){
					if (( this.euclideanDist(data[0], data[1], Base.finalDestination[0],Base.finalDestination[1]) <= 5)&&(this.M3==-1)) this.M3 = this.step;
				}
				
				this.step++;
				indexStep = this.step;
				this.M2 = this.M2 + this.C;
				this.C = -1;
				if (this.step>89) indexStep = (int) (indexStep - 80*Math.floor((this.step-10)/80));
				
				if (!this.doCheck) break;
				dist = this.euclideanDist(this.realRobotPositions.get(indexStep)[0], this.realRobotPositions.get(indexStep)[1], data[0], data[1]);
				if (dist>=0.5){
					StyleConstants.setForeground(this.currentStyle, Color.red);
					result = result+"\tinstead of ("+this.realRobotPositions.get(indexStep)[0]+";"+this.realRobotPositions.get(indexStep)[1]+")";
				} else if (dist>=0.05){
					StyleConstants.setForeground(this.currentStyle, Color.gray);
				}
				break;
			case DataChecker.stopInfo:
				if (this.scenario == 1){
					if (!this.doCheck) break;
					result="\n\n|**************Experiment's Informations*****************|\n";
					result=result+"After "+this.step+" step"+this.isPlurial(this.step)+", we have "+this.dataError+" error"+this.isPlurial(this.dataError);
					if (this.dataError==0) result = result+". Congratulations! no error detected!\n";
					else result = result+".\n";
					result=result+"Root Mean Square Error = "+df.format(this.rootMeanSquareError)+"\n";
					result=result+"|************************************************************|\n\n \n";
				} else {
					result="\n\n|**************Experiment's Informations*****************|\n";
					result=result+"Experiment time = "+this.step+" step"+this.isPlurial(this.step)+"\n";
					if (this.M3 != -1){
						result=result+"Congratulations! The robot has reached its destination point ("+Base.finalDestination[0]+","+Base.finalDestination[1]+") at the step "+this.M3+"\n";
					} else {
						result=result+"Too bad! The robot hasn't (still?) reached its destination point ("+Base.finalDestination[0]+","+Base.finalDestination[1]+")\n";
					}
					double[] temp;
					for (int i=0;i<this.landmarks.size();i++){
						temp = this.landmarks.get(i);
						result=result+"Minimal distance to landmark "+df.format(temp[0])+" is "+df.format(temp[3])+" unit"+this.isPlurial(temp[3])+"\n";
					}
					if (this.M4[0] == -1){
						result = result + "The robot has never been sensed. Congratulations!\n";
					} else {
						result = result + "The robot has been sensed for the first time at the step "+this.M4[0]+" by the node "+this.M4[1]+"\n";
					}
					result=result+"|************************************************************|\n \n";	
				}
				
				
				break;
			case DataChecker.ekf:
				result="[EKF]\tX= "+df.format(data[0])+"\tY= "+df.format(data[1]);
				break;
			case DataChecker.displayM1:
				for (int i=0;i<10;i++){
					tempLandmark = this.getLandmark(i);
					if ( tempLandmark!=null ){
						result = result + "M1,"+df.format(tempLandmark[0])+" = "+df.format(tempLandmark[3])+" unit"+this.isPlurial(this.M2/this.step)+"\n";
					}
				}	 
				break;
			case DataChecker.displayM2:
				if (this.step!=0){
					result = "M2("+this.step+") = "+df.format(this.M2/this.step)+" unit"+this.isPlurial(this.M2/this.step);
				}
				break;
			case DataChecker.displayM3:
				if (this.M3 != -1){
					result = "M3 = "+this.M3+"";
				} else {
					result = "M3 = -1";
				}
				break;
			case DataChecker.displayM4:
				if (this.M4[0] == -1){
					result = "M4 = -1";
				} else {
					result = "M4 = "+this.M4[0]+" by the node "+this.M4[1]+"";
				}
				break;
			default:
				break;
		}
		return result+"\n";
	}
	
	public void saveInfo(){
		FileOutputStream fout;
		OutputStreamWriter out;
		String name = "metrics_nb"+this.landmarks.size()+"_err"+this.errorFactor;
		String data = null;
		double [] tempLandmark= null;
		try
		{
		    // Open an output stream
		    fout = new FileOutputStream (name+".txt",true);
		    out = new OutputStreamWriter(fout, "UTF-8");
		    
		    data = "(s"+this.step+")\t";
		    for (int i=0;i<20;i++){
		    	tempLandmark = this.getLandmark(i);
		    	if (tempLandmark != null){
			    	if (data.charAt(data.length()-1)!=';') data = "[M1,"+df.format(tempLandmark[0])+" "+df.format(tempLandmark[3]);
			    	else data = data + ";M1,"+df.format(tempLandmark[0])+" "+df.format(tempLandmark[3]);
		    	}
		    }

		    data = data + "]\t" + df.format(this.M2)+"\t"+df.format(this.M3)+"\t"+df.format(this.M4[0])+" ("+df.format(this.M4[1])+")\n";
			
			out.append(data);

		    // Close our output stream
		    out.close();		
		}
		// Catches any error conditions
		catch (IOException e)
		{
			System.err.println ("Unable to write to file");
			System.exit(-1);
		}
		
	}

	public void clear() {
		this.step = 0;
		this.indexStep = 0;
		this.currentRobotPosition = new double [2];
		this.dataError = 0;
		this.rootMeanSquareError = 0;
		this.M2 = 0;
		this.M3 = -1;
		this.M4 = new int[]{-1,0};
		this.landmarks = new Vector<double[]>(5,1);
		this.initRobotPositions();
		this.initRealDestination();
		
	}

}
